# -*- coding: utf-8 -*-
"""
Created on Tue Apr 16 11:48:30 2013

@author: Maxim
"""
import design
from performance import TakeoffLanding
import FlightConditions
from numpy import zeros,array
from scipy.optimize import fmin_slsqp

def normalize(x,bnds):
    xnorm = zeros(len(x))
    for i,xx in enumerate(x):
        lb = bnds[i][0]
        ub = bnds[i][1]
        xnorm[i] = (2.0*xx-(lb+ub))/(ub-lb)
    return xnorm

def denormalize(xnorm,bnds):
    x = zeros(len(xnorm))
    for i,xx in enumerate(xnorm):
        lb = bnds[i][0]
        ub = bnds[i][1]
        x[i] = 0.5*(xnorm[i]*(ub-lb)+(lb+ub))
    return x

def stability_optimization():
    baselineAircraft = 'V0510'
    ac = design.load(baselineAircraft)
    # input section
    dx = 0.0
    targets = zeros(15)
    parameters = zeros(15)
    targets[0] = 0.08    # static margin min
    targets[1] = 0.04    # CnBeta
    targets[2] = -0.02   # ClBeta
    targets[3] = 20.0    # elevator deflection for approach
    targets[4] = 30.0    # elevator deflection for takeoff rotation
    targets[5] = 0.35    # short period damping ratio min
    targets[6] = 2.00    # short period damping ratio max
    targets[7] = 2.00    # short period natural frequency min
    targets[8] = 10.0    # short period natural frequency max
    targets[9] = 1.00    # dutch roll natural frequency min
    targets[10] = 0.08   # dutch roll damping ratio min
    targets[11] = 0.4    # vertical tail volume ratio

    parameters[0] = 0.8  # takeoff rotation stall speed fraction
    parameters[1] = 0.7  # takeoff lift coefficient
    parameters[2] = 1.2  # flare load factor
    parameters[3] = 2.2  # Clmax takeoff
    parameters[4] = 35.0 # landing flap deflection
    parameters[5] = 0.0  # takeoff flap deflection
    parameters[6] = 0.9*parameters[3] # Clmax at landing
    parameters[7] = 1500.0 # cruise altitude
    
    grossMass = ac.mass()
    fieldAtm = FlightConditions.ISAtmosphere(0.0)
    g = 9.81
    VstallLdg = (2.0*grossMass*g/fieldAtm.density/ac.wing.area/parameters[6])**0.5
    VstallTO  = (2.0*grossMass*g/fieldAtm.density/ac.wing.area/parameters[3])**0.5
    
    parameters[8] = VstallLdg
    parameters[9] = VstallTO

    def sizing_objective(x,a,b,c,bnds):
        x = normalize(x,bnds)
        f = x[0]+x[1]
        return 10.0*f
    
    def constraints(x,aircraft,targets,parameters,bnds):

        htLoc  = x[0]
        vtSpan = x[1]
        ht = aircraft.hStab.aapex
        
        aircraft.set_hstab_position(array([htLoc,ht[1],ht[2]]))
        curOffset = aircraft.vStab.segmentOffsets[0]
        curSpan   = aircraft.vStab.segmentSpans[1]
        newOffset = vtSpan/curSpan*curOffset
        
        aircraft.set_vstab_span_by_index(vtSpan,1)
        aircraft.set_vstab_offset_by_index(newOffset,0)
        
        
        takeoff = TakeoffLanding()
        elevTO = takeoff.takeoff_rotation(aircraft,0.0,parameters[0],flapDefl=parameters[5])
        
        Vland = 1.2*parameters[8]
        landing = aircraft.analyze_aero_trim(Vland,0.0,parameters[2],0.0,parameters[4])
        elevLdg = landing.elevator
        cruise = aircraft.analyze_aero_trim(altitude=parameters[7])
        
        np = cruise.xNP
        cg = aircraft.get_CG()
        sm = (np-cg[0])/aircraft.wing.MAC
        CnBeta = cruise.derivs.Cnb
        ClBeta = cruise.derivs.Clb
        sp_freq = cruise.dynamicStability.shortPeriod.Wn
        sp_damp = cruise.dynamicStability.shortPeriod.Z
        dr_freq = cruise.dynamicStability.dutchRoll.Wn
        dr_damp = cruise.dynamicStability.dutchRoll.Z
        out = ''
        for xx in x:
            out += '%.4f  '%xx
        out +='| '
        out += 'SM: %.4f  '%sm
        out += 'Clb: %.4f  '%ClBeta
        out += 'Cnb: %.4f  '%CnBeta
        out += 'sp_W: %.4f  '%sp_freq
        out += 'sp_Z: %.4f  '%sp_damp
        out += 'dr_W: %.4f  '%dr_freq
        out += 'dr_Z: %.4f  '%dr_damp
        print out
        g = zeros(12)
        g[0] = sm - targets[0]
        g[1] = (CnBeta - targets[1])*10.0
        g[2] = (targets[2] - ClBeta)*10.0
        g[3] = targets[3] - elevLdg
        g[4] = targets[4] - elevTO
        g[5] = sp_damp - targets[5]
        g[6] = targets[6] - sp_damp
        g[7] = sp_freq - targets[7]
        g[8] = targets[8] - sp_freq
        g[9] = dr_freq - targets[9]
        g[10] = dr_damp - targets[10]
        g[11] = 0.20 - sm
        return g

    bnds = ((5.4,5.6),(1.2,1.6))
    x0 = array([5.4,1.3])

    
    aapex = ac.vStab.aapex
    print aapex
    ac.set_vstab_position([aapex[0]+dx,aapex[1],aapex[2]])
    args = ac,targets,parameters,bnds
    rslt = fmin_slsqp(sizing_objective,x0, bounds=bnds,
                    f_ieqcons=constraints,epsilon=1.0e-4,args=args,acc=1.0e-4)
    print rslt
    #ac.mass.display()
    print ac.vStab.area
    ac.display()


if __name__=="__main__":
    stability_optimization()
